/**
 ******************************************************************************
 * Copyright (c) 2022 - ~, SCUT-RobotLab Development Team
 * @file    FootSwingTrajectory.cpp
 * @author
 * @brief   Code for .
 * @date   2022-02-28
 * @version 1.0
 * @par Change Log:
 * <table>
 * <tr><th>Date        <th>Version  <th>Author     <th>Description
 * </table>
 *
 ******************************************************************************
 * @attention
 *
 * if you had modified this file, please make sure your code does not have many
 * bugs, update the version Number, write dowm your name and the date, the most
 * important is make sure the users will have clear and definite understanding
 * through your new brief.
 *
 * <h2><center>&copy; Copyright (c) 2022 - ~,SCUT-RobotLab Development Team.
 * All rights reserved.</center></h2>
 ******************************************************************************
 */
#include <common/Controllers/FootSwingTrajectory.h>
#include <common/Utilities/Interpolation.h>

/*!
 * 用bezier曲线计算脚的摆动轨迹
 * @tparam T
 * @param phase
 * @param swingTime
 * （1）这里的求导方式不是求得位置轨迹后对其进行求导，而是在计算的时候已经对三阶的贝塞尔曲线公式进行求导了
 *（2）这里的起点和目标点不是三维的，而是一个标量，足端取的是Z轴的高度坐标
 *（3）这里的轨迹规划是先从起点到抬腿高度点（抬脚）进行规划，再从抬腿高度点到目标点（落脚）进行规划
 *（4）该工程的起点、目标点、高度、和控制占空比因子(0,1)是从外部用API给定的
 */
template <typename T>
void FootSwingTrajectory<T>::computeSwingTrajectoryBezier(T phase, T swingTime)
{
  _p = Interpolate::cubicBezier<Vec3<T>>(_p0, _pf, phase);                                           // y0和yf之间的线性插值。x在0和1之间在两个值之间插值
  _v = Interpolate::cubicBezierFirstDerivative<Vec3<T>>(_p0, _pf, phase) / swingTime;                // y0和yf之间的三次bezier插值导数。x在0和1之间
  _a = Interpolate::cubicBezierSecondDerivative<Vec3<T>>(_p0, _pf, phase) / (swingTime * swingTime); // y0和yf之间的三次bezier插值导数。x在0到之间1

  T zp, zv, za;
  if (phase < T(0.5)) //相位小于0.5，抬脚阶段
  {
    zp = Interpolate::cubicBezier<T>(_p0[2], _p0[2] + _height, phase * 2);
    zv = Interpolate::cubicBezierFirstDerivative<T>(_p0[2], _p0[2] + _height, phase * 2) * 2 / swingTime;
    za = Interpolate::cubicBezierSecondDerivative<T>(_p0[2], _p0[2] + _height, phase * 2) * 4 / (swingTime * swingTime);
  }
  else //相位大于0.5，放脚阶段
  {
    zp = Interpolate::cubicBezier<T>(_p0[2] + _height, _pf[2], phase * 2 - 1);
    zv = Interpolate::cubicBezierFirstDerivative<T>(_p0[2] + _height, _pf[2], phase * 2 - 1) * 2 / swingTime;
    za = Interpolate::cubicBezierSecondDerivative<T>(_p0[2] + _height, _pf[2], phase * 2 - 1) * 4 / (swingTime * swingTime);
  }

  _p[2] = zp;
  _v[2] = zv;
  _a[2] = za;
}

template <typename T>
void FootSwingTrajectory<T>::computeSwingTrajectoryBezier2(T phase, T swingTime)
 {
    T tempz ,tempzv;
    // Z
    if (phase < T(0.5)) {
        tempz = Interpolate::cubicBezier<T>(0, _height, phase*2);
        tempzv=Interpolate::cubicBezierFirstDerivative<T>(0, _height, phase*2) * 2 / swingTime;
    }
    else
    {
         tempz= Interpolate::cubicBezier<T>(_height, 0, phase*2-1);
         tempzv=Interpolate::cubicBezierFirstDerivative<T>(_height, 0, phase*2-1) * 2 / swingTime;
    }
    //std::cout<<temp<<std::endl;
    // X
     _p(0) = Interpolate::cubicBezier<T>(_p0(0), _pf(0), phase );
     _v(0) = Interpolate::cubicBezierFirstDerivative<T>(_p0(0), _pf(0),phase) / swingTime;
    //Y
    _p(1) =  Interpolate::cubicBezier<T>(_p0(1), _pf(1), phase );
    _v(1) = Interpolate::cubicBezierFirstDerivative<T>(_p0(1), _pf(1),phase) / swingTime;
     //Z
    _p(2)=_p0(2)+tempz;
    _v(2)= tempzv;
     //std::cout<< _p(2)<<std::endl;
}

template class FootSwingTrajectory<double>;
template class FootSwingTrajectory<float>;

/************************ COPYRIGHT(C) SCUT-RobotLab Development Team **************************/
